//
// Created by yu on 2023/9/23.
//
#include <ros/ros.h>
#include <eigen3/Eigen/Dense>

#include "imuData.h"
#include "mi-motor.h"
#include "lk-motor.h"
#include "motor_control.h"
#include "callBacks.h"
#include "msg_node/motorData.h"
#include "msg_node/plot.h"
#include "RobotMath.h"

using namespace Eigen;
using namespace std;

int main(int argc, char *argv[])
{
////////////////////////////////////////////// ros初始化 /////////////////////////////////////////////////////////
    ros::init(argc, argv, "main_control");
    ros::NodeHandle nh;

/// 订阅
    ros::Subscriber imuData = nh.subscribe("wit/imu", 1, imuCallBack);
    ros::Subscriber magData = nh.subscribe("wit/mag", 1, magCallBack);
    ros::Subscriber receiver = nh.subscribe("receiver", 1, callBack::receiverCallBack);

/// 发布
    ros::Publisher motorData = nh.advertise<msg_node::motorData>("motorData", 1);
    ros::Publisher plotData = nh.advertise<msg_node::plot>("plotData", 1);

/// 循环速度
    ros::Rate loop_rate(150);

////////////////////////////////////////////// 其他初始化 /////////////////////////////////////////////////////////

/// 设置中文输出格式
    setlocale(LC_CTYPE, "zh_CN.utf8");
/// 读取配置文件
    Motor.readYaml();
/// 所有电机开始运行
    Motor.controlBegin();
/// 腿复位
    Motor.legReset();
    ROS_INFO("\033[32mKernel opened successfully...\033[0m");

    while (nh.ok() && CANserialPort.isOpen())
    {

        if (receiver_data.channel5 >= 0.9)
        {
            ROS_INFO("%lf",ImuData.accelerate_z);
        }
        else
        {
            Motor.controlStop();
            Motor.pidReset();
        }

        ros::spinOnce();
        loop_rate.sleep();

    }
}
